#ifndef TESTREFSMOOTHER_H
#define TESTREFSMOOTHER_H

#include "ros/ros.h"
#include "global_refer.h"
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <thread>
#include <boost/thread.hpp>
#include <string>
#include <eigen3/Eigen/Dense>
#include "lqr_controller.h"
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <waypoint_msgs/WaypointArray.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <tf/tf.h>
#include "std_msgs/String.h"
#include "mpc_controller.h"

#define YELLOW "\033[0;33m"
#define RESET "\033[0m" // 重置颜色

using namespace std;
using namespace Eigen; 
class TestRefSmoother
{
public:
  TestRefSmoother();
  ~TestRefSmoother();
  void processAndPublishData();
  // void initialpose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
  void move_base_simple_goal(const geometry_msgs::PoseStamped::ConstPtr &msg);
  void Odom(const nav_msgs::Odometry &msg);
  void AmclPose(const geometry_msgs::PoseWithCovarianceStamped &msg);
  void Theta(const geometry_msgs::PoseArray &theta);
  void Kappa(const geometry_msgs::PoseArray &kappa);
  void Acceleration(const geometry_msgs::PoseArray &acceleration);
  void Traj_speed(const waypoint_msgs::WaypointArray::ConstPtr &msg);
  bool Vehical_Odom_Stop(const geometry_msgs::Pose &goal_point, nav_msgs::Odometry &car_locat);
  bool Vehical_Locat_Stop(const geometry_msgs::Pose &goal_point, geometry_msgs::PoseWithCovarianceStamped &locat);
  void LocalStart(const std_msgs::String::ConstPtr &go);
  void startThread();
  void public_control_thread();

private:
  Global_refer Gl;
  LqrController Lqr;
  MpcController Mpc;
  std::vector<double> speeds, thetas, kappas, accelerations;
  std::thread task_thread_;
  boost::thread *public_control_thread_;
  ros::NodeHandle nh;
  // ros::Publisher hdmap_way_point_;
  ros::Publisher control_data_pub_;
  // ros::Subscriber initialpose_;
  ros::Subscriber move_base_simple_goal_;
  ros::Subscriber theta_;
  ros::Subscriber kappa_;
  ros::Subscriber acceleration_;
  ros::Subscriber traj_speed_;
  ros::Subscriber Start_local_;
  ros::Subscriber odom_;
  ros::Subscriber AmclPose_;


  // geometry_msgs::Pose init_pose_;
  geometry_msgs::Pose goal_pose_;
  geometry_msgs::PoseStamped pose_stamp;
  geometry_msgs::PoseArray local_theta_;
  // geometry_msgs::PoseStamped hdmap_way_points_pose;
  nav_msgs::Path dynamic_points;
  // nav_msgs::Path hdmap_way_points_msg;
  geometry_msgs::Vector3Stamped msg_ros;
  nav_msgs::Odometry car_odom_;
  geometry_msgs::PoseWithCovarianceStamped car_locat_pose_ ;


  Eigen::MatrixXd hdmap_way_points;

  std::string csv_file_path;
  string Start_local_planning;     // 判断局部规划是否开始
  bool initialpose_set;   // 起点接受成功信号
  bool move_base_simple_goal_set; // 终点接受成功标志
  bool hasProcessed;
  bool is_vehical_stop_set;
  double turn_angle;        // 车转角
  double goal_distanse;
  // int out_index_;
  double out_index_;
  double car_speed;         // 车速度
  double global_odom_x ;
  double global_odom_y ;
  double global_odom_yaw ;

};

#endif // TESTREFSMOOTHER_H
